#include "PathFinder.h"
#include "ResLoader.h"
#include <stdio.h>
PathFinder::PathFinder(dtNavMesh* navMesh,dtNavMeshQuery* navQuery)
	:m_navQuery(navQuery),
	m_navMesh(navMesh)
{
	m_polyPickExt[0] = 2;
	m_polyPickExt[1] = 4;
	m_polyPickExt[2] = 2;
	m_filter.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED);
	m_filter.setExcludeFlags(0);
	// Change costs.
	m_filter.setAreaCost(SAMPLE_POLYAREA_GROUND, 1.0f);
	m_filter.setAreaCost(SAMPLE_POLYAREA_WATER, 10.0f);
	m_filter.setAreaCost(SAMPLE_POLYAREA_ROAD, 1.0f);
	m_filter.setAreaCost(SAMPLE_POLYAREA_DOOR, 1.0f);
	m_filter.setAreaCost(SAMPLE_POLYAREA_GRASS, 2.0f);
	m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP, 1.5f);
}

PathFinder::~PathFinder()
{
	m_navMesh = NULL;
	dtFreeNavMeshQuery(m_navQuery);
	//SAFE_DELETE(m_navQuery);
}


bool PathFinder::Find(float& sposX, float& sposY, float& sposZ, float& eposX, float& eposY, float& eposZ)
{	
	Reset();
	if (!m_navMesh) 
	{ 
		//printf("PathFinder::Find m_navMesh is null! \n");
		return false; 
	}

	dtPolyRef m_startRef = 0;
	dtPolyRef m_endRef = 0;
	float m_spos[3];
	float m_epos[3];
	dtStatus result = DT_SUCCESS;
	m_spos[0] = sposX;
	m_spos[1] = sposY;
	m_spos[2] = sposZ;

	m_epos[0] = eposX;
	m_epos[1] = eposY;
	m_epos[2] = eposZ;
	result = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
	if (dtStatusFailed(result))
	{
		//printf("PathFinder::Find failed, findNearestPoly result: %u\n",result);
		return false;
	}

	result = m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
	if (dtStatusFailed(result))
	{
		//printf("PathFinder::Find failed, findNearestPoly result: %u\n", result);
		return false;
	}
	//printf("PathFinder::Find m_navMesh m_startRef = %ld ,m_startRef = %ld \n", m_startRef, m_endRef);
	if (m_startRef && m_endRef)
	{
		
		/*int m_npolys = 0;
		int m_nstraightPath = 0;*/
		result = m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);

		if (dtStatusFailed(result))
		{
			//printf("PathFinder::Find failed, findPath result: %u\n", result);
			return false;
		}

		//printf("PathFinder::Find m_navMesh m_npolys = %ld \n", m_npolys);
		if (m_npolys)
		{
			float epos[3];
			dtVcopy(epos, m_epos);
			if (m_polys[m_npolys - 1] != m_endRef)
				m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, 0);

			result = m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
				m_Path, m_PathFlags,
				m_PathPolys, &m_nstraightPath, MAX_POLYS, 0);

			if (dtStatusFailed(result))
			{
				//printf("PathFinder::Find failed, findStraightPath result: %u\n", result);
				return false;
			}

			if (m_nstraightPath) 
			{
				//printf("PathFinder::Find m_nstraightPath %ld\n", m_nstraightPath);
				//for (int i = 0;i<m_nstraightPath;i++)
				//{
				//	printf("PathFinder::Find m_nstraightPath %f %f %f\n", m_Path[i*3], m_Path[i*3+1],m_Path[i*3+2]);
				//}
				return true;
			}
		}
	}
	return false;
}

bool PathFinder::CalcNear(float& sposX,float& sposY, float& sposZ)
{
	
	Reset();
	if (!m_navMesh) 
	{ 
		printf("PathFinder::CaleNear m_navMesh is null! \n");
		m_nearCount=0;
		return false; 
	}

	dtPolyRef m_startRef = 0;
	dtPolyRef m_endRef = 0;
	float m_spos[3];
	m_spos[0] = sposX;
	m_spos[1] = sposY;
	m_spos[2] = sposZ;

	dtStatus result = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, m_nearPos);

	//printf("PathFinder::CaleNear m_navMesh m_startRef = %u ,m_startRef = %u result: %u \n", m_startRef, m_endRef, result);
	//printf("PathFinder::CaleNear result: %u spos: %f,%f,%f epos: %f,%f,%f\n", result,sposX,sposY,sposZ,m_nearPos[0],m_nearPos[1],m_nearPos[2]);

	if (dtStatusFailed(result))
	{
		//printf("PathFinder::CaleNear failed, findStraightPath result: %u\n", result);
		return false;
	}

	if (m_startRef && m_startRef)
	{
		//printf("PathFinder::CalcNear is ok\n");
		m_nearCount=1;
		return true;
	} else {
		//printf("PathFinder::CalcNear is fail\n");
		m_nearCount=0;
	}

	return false;
}

float* PathFinder::GetNear(int* count)
{
	*count = m_nearCount;
	return m_nearPos;
}

bool PathFinder::CalcRaycast(float& sposX, float& sposY, float& sposZ, float& eposX, float& eposY, float& eposZ)
{

	Reset();
	if (!m_navMesh)
	{
		//printf("PathFinder::CalcRaycast m_navMesh is null! \n");
		m_nearCount = 0;
		return false;
	}

	dtPolyRef m_startRef = 0;
	dtPolyRef m_endRef = 0;
	float m_spos[3] = { sposX ,sposY,sposZ};
	float m_epos[3] = { eposX ,eposY,eposZ };
	float t = 0;
	float m_hitNormal[3];
	dtStatus result = 0;

	result = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
	if (dtStatusFailed(result)) {
		//printf("PathFinder::CalcRaycast fail, findNearestPoly fail, result: %u\n",result);
		return false;
	}

	result = m_navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
	if (dtStatusFailed(result)) {
		//printf("PathFinder::CalcRaycast fail, raycast fail, result: %u\n", result);
		return false;
	}
	if (t > 1)
	{
		// No hit
		dtVcopy(m_hitPos, m_epos);
		return false;
	}
	else
	{
		// Hit
		dtVlerp(m_hitPos, m_spos, m_epos, t);
		m_hitCount = 1;
		return true;
	}
}

float *PathFinder::GetPath(int* count)
{
	*count = m_nstraightPath;
	return m_Path;
}

float* PathFinder::GetRaycast(int* count)
{
	*count = m_hitCount;
	return m_hitPos;
}

void PathFinder::Reset()
{
	m_nstraightPath = 0;
	m_npolys = 0;
	m_nearCount=0;
	m_hitCount = 0;
}


